{
 "cells": [
  {
   "attachments": {},
   "cell_type": "markdown",
   "id": "1add6554",
   "metadata": {},
   "source": [
    "# Robotics, Vision & Control 3e: for Python\n",
    "## Chapter 8: Manipulator Velocity\n",
    "\n",
    "Copyright (c) 2021- Peter Corke"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "387860c2",
   "metadata": {},
   "outputs": [],
   "source": [
    "try:\n",
    "    from google.colab import output\n",
    "    print('Running on CoLab')\n",
    "    output.enable_custom_widget_manager()\n",
    "    !pip install ipympl\n",
    "    !pip install roboticstoolbox-python>=1.0.2\n",
    "    !pip install --no-deps rvc3python\n",
    "    COLAB = True\n",
    "\n",
    "except ModuleNotFoundError:\n",
    "    COLAB = False\n",
    "\n",
    "from IPython.core.interactiveshell import InteractiveShell\n",
    "InteractiveShell.ast_node_interactivity = \"last_expr_or_assign\"\n",
    "from IPython.display import HTML\n",
    "\n",
    "%matplotlib widget\n",
    "import matplotlib.pyplot as plt\n",
    "\n",
    "# add RTB examples folder to the path\n",
    "import sys, os.path\n",
    "import RVC3 as rvc\n",
    "sys.path.append(os.path.join(rvc.__path__[0], 'models'))\n",
    "\n",
    "# helper function to run bdsim in a subprocess and transfer results using a pickle file\n",
    "import pickle\n",
    "def run_shell(tool, **params):\n",
    "    global out\n",
    "    pyfile = os.path.join(rvc.__path__[0], \"models\", tool+\".py\")\n",
    "    cmd = f\"python {pyfile} -H +a -o\"\n",
    "    for key, value in params.items():\n",
    "        cmd += f' --global \"{key}={value}\"'\n",
    "    print(cmd)\n",
    "    os.system(cmd)\n",
    "    with open(\"bd.out\", \"rb\") as f:\n",
    "        out = pickle.load(f)\n",
    "\n",
    "# ------ standard imports ------ #\n",
    "import numpy as np\n",
    "from scipy import linalg\n",
    "import math\n",
    "from math import pi\n",
    "np.set_printoptions(\n",
    "    linewidth=120, formatter={\n",
    "        'float': lambda x: f\"{0:8.4g}\" if abs(x) < 1e-10 else f\"{x:8.4g}\"})\n",
    "np.random.seed(0)\n",
    "from spatialmath import *\n",
    "from spatialmath.base import *\n",
    "from roboticstoolbox import *\n"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "56907a14",
   "metadata": {},
   "source": [
    "# 8.1 Manipulator Jacobian\n"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "4f4035ca",
   "metadata": {},
   "source": [
    "## 8.1.1 Jacobian in the World Coordinate Frame\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "884d0c78",
   "metadata": {},
   "outputs": [],
   "source": [
    "import sympy\n",
    "a1, a2 = sympy.symbols(\"a1, a2\")\n",
    "e = ERobot2(ET2.R() * ET2.tx(a1) * ET2.R() * ET2.tx(a2))"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "0585f758",
   "metadata": {},
   "outputs": [],
   "source": [
    "q = sympy.symbols(\"q:2\")"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "b180da83",
   "metadata": {},
   "outputs": [],
   "source": [
    "TE = e.fkine(q);"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "95cb4370",
   "metadata": {},
   "outputs": [],
   "source": [
    "p = TE.t"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "7f261bf7",
   "metadata": {},
   "outputs": [],
   "source": [
    "J = sympy.Matrix(p).jacobian(q)\n",
    "J.shape"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "84fd65e1",
   "metadata": {},
   "outputs": [],
   "source": [
    "ur5 = models.URDF.UR5();\n",
    "J = ur5.jacob0(ur5.q1)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "f7293fad",
   "metadata": {},
   "outputs": [],
   "source": [
    "# ur5.teach(ur5.q1, backend=\"pyplot\")\n",
    "ur5.plot(ur5.q1, backend=\"pyplot\")"
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "id": "2dea6028",
   "metadata": {},
   "source": [
    "A robot model created from a URDF file uses Swift by default for visualization.  We explicitly set the backed to `\"plplot\"` but that does not support teaching in the Jupyter environment."
   ]
  },
  {
   "cell_type": "markdown",
   "id": "0301302f",
   "metadata": {},
   "source": [
    "## 8.1.2 Jacobian in the End-Effector Coordinate Frame\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "9cbe17cc",
   "metadata": {},
   "outputs": [],
   "source": [
    "ur5.jacobe(ur5.q1)"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "c308c94b",
   "metadata": {},
   "source": [
    "## 8.1.3 Analytical Jacobian\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "a865bc3d",
   "metadata": {},
   "outputs": [],
   "source": [
    "rotvelxform((0.1, 0.2, 0.3), representation=\"rpy/xyz\")"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "c508b873",
   "metadata": {},
   "outputs": [],
   "source": [
    "ur5.jacob0_analytical(ur5.q1, \"rpy/xyz\");"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "d1649a66",
   "metadata": {},
   "source": [
    "# 8.2 Application: Resolved-Rate Motion Control\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "e05961c8",
   "metadata": {},
   "outputs": [],
   "source": [
    "if COLAB:\n",
    "  %run -m RRMC -H -g  # no graphics\n",
    "else:\n",
    "  run_shell(\"RRMC\")\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "5751183f",
   "metadata": {},
   "outputs": [],
   "source": [
    "t = out.clock0.t;\n",
    "q = out.clock0.x;"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "c0234351",
   "metadata": {},
   "outputs": [],
   "source": [
    "xplot(t, q[:, :3], stack=True);"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "aa5fb99a",
   "metadata": {},
   "outputs": [],
   "source": [
    "from roboticstoolbox.models.DH import Puma560  # need to explicitly import this because we run RRMC in a subprocess\n",
    "puma = Puma560()\n",
    "\n",
    "Tfk = puma.fkine(q);"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "95e0db86",
   "metadata": {},
   "outputs": [],
   "source": [
    "xplot(out.clock0.t, Tfk.t, stack=True);"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "73744245",
   "metadata": {},
   "outputs": [],
   "source": [
    "if COLAB:\n",
    "  %run -m RRMC2 -H -g  # no graphics\n",
    "else:\n",
    "  run_shell(\"RRMC2\")\n"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "5f3f9fdd",
   "metadata": {},
   "source": [
    "# 8.3 Jacobian Condition and Manipulability\n"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "6c4a9129",
   "metadata": {},
   "source": [
    "## 8.3.1 Jacobian Singularities\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "7591036a",
   "metadata": {},
   "outputs": [],
   "source": [
    "J = ur5.jacob0(ur5.qz)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "5add00bc",
   "metadata": {},
   "outputs": [],
   "source": [
    "np.linalg.det(J)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "634c9b2a",
   "metadata": {},
   "outputs": [],
   "source": [
    "np.linalg.matrix_rank(J)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "071117fa",
   "metadata": {},
   "outputs": [],
   "source": [
    "jsingu(J)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "c616078b",
   "metadata": {},
   "outputs": [],
   "source": [
    "qns = np.full((6,), np.deg2rad(5))"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "a6b6a273",
   "metadata": {},
   "outputs": [],
   "source": [
    "J = ur5.jacob0(qns);"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "b280dbe7",
   "metadata": {},
   "outputs": [],
   "source": [
    "qd = np.linalg.inv(J) @ [0, 0, 0, 0.1, 0, 0]"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "89db1133",
   "metadata": {},
   "outputs": [],
   "source": [
    "np.linalg.det(J)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "ec8a6150",
   "metadata": {},
   "outputs": [],
   "source": [
    "np.linalg.cond(J)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "9867cb16",
   "metadata": {},
   "outputs": [],
   "source": [
    "qd = np.linalg.inv(J) @ [0, 0.1, 0, 0, 0, 0]"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "55e099ed",
   "metadata": {},
   "source": [
    "## 8.3.2 Velocity Ellipsoid and Manipulability\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "85699a14",
   "metadata": {},
   "outputs": [],
   "source": [
    "planar2 = models.ETS.Planar2();"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "2b1d1aef",
   "metadata": {},
   "outputs": [],
   "source": [
    "# planar2.teach(np.deg2rad([30, 40]), vellipse=True);\n",
    "planar2.plot(np.deg2rad([30, 40]), vellipse=True);"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "0b2b625c",
   "metadata": {},
   "outputs": [],
   "source": [
    "J = ur5.jacob0(ur5.q1);"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "9d0720ea",
   "metadata": {},
   "outputs": [],
   "source": [
    "Jt = J[:3, :];  # first 3 rows"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "f0e2cae3",
   "metadata": {},
   "outputs": [],
   "source": [
    "E = np.linalg.inv(Jt @ Jt.T)\n",
    "plot_ellipsoid(E);"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "9acf63e0",
   "metadata": {},
   "outputs": [],
   "source": [
    "e, _ = np.linalg.eig(E);\n",
    "radii = 1 / np.sqrt(e)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "672ba700",
   "metadata": {},
   "outputs": [],
   "source": [
    "J = ur5.jacob0(np.full((6,), np.deg2rad(1)));\n",
    "Jr = J[3:, :];  # last 3 rows\n",
    "E = np.linalg.inv(Jr @ Jr.T);\n",
    "plot_ellipsoid(E);\n",
    "plt.gca().set_aspect(\"equal\")"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "ff8c0b31",
   "metadata": {},
   "outputs": [],
   "source": [
    "e, x = np.linalg.eig(E);\n",
    "radii = 1 / np.sqrt(e)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "1d499c7c",
   "metadata": {},
   "outputs": [],
   "source": [
    "x[:, 0]"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "a09a4103",
   "metadata": {},
   "outputs": [],
   "source": [
    "ur5.vellipse(qns, \"rot\");"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "b7934225",
   "metadata": {},
   "outputs": [],
   "source": [
    "ur5.manipulability(ur5.q1)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "47384701",
   "metadata": {},
   "outputs": [],
   "source": [
    "ur5.manipulability(ur5.qz)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "ee36a29a",
   "metadata": {},
   "outputs": [],
   "source": [
    "ur5.manipulability(ur5.qz, axes=\"both\")"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "2b2825d4",
   "metadata": {},
   "source": [
    "## 8.3.4 Dealing with a non-square Jacobian\n"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "83cc97b6",
   "metadata": {},
   "source": [
    "### 8.3.4.1 Jacobian for Under-Actuated Robot\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "022d4532",
   "metadata": {},
   "outputs": [],
   "source": [
    "planar2 = models.ETS.Planar2();\n",
    "qn = [1, 1];"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "29fb7a62",
   "metadata": {},
   "outputs": [],
   "source": [
    "J = planar2.jacob0(qn)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "dbef7734",
   "metadata": {},
   "outputs": [],
   "source": [
    "xd_desired = [0.1, 0.2, 0];"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "f010f48e",
   "metadata": {},
   "outputs": [],
   "source": [
    "qd = np.linalg.pinv(J) @ xd_desired"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "17d0410d",
   "metadata": {},
   "outputs": [],
   "source": [
    "J @ qd"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "7de7707c",
   "metadata": {},
   "outputs": [],
   "source": [
    "np.linalg.norm(xd_desired - J @ qd)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "be21cd11",
   "metadata": {},
   "outputs": [],
   "source": [
    "Jxy = J[:2, :];\n",
    "qd = np.linalg.inv(Jxy) @ xd_desired[:2]"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "ea24f4b5",
   "metadata": {},
   "outputs": [],
   "source": [
    "xd = J @ qd"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "fc9524f7",
   "metadata": {},
   "outputs": [],
   "source": [
    "np.linalg.norm(xd_desired - J @ qd)"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "c24eb797",
   "metadata": {},
   "source": [
    "### 8.3.4.2 Jacobian for Overactuated Robot\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "a6b05793",
   "metadata": {},
   "outputs": [],
   "source": [
    "panda = models.ETS.Panda();\n",
    "TE = SE3.Trans(0.5, 0.2, -0.2) * SE3.Ry(pi);\n",
    "# sol = panda.ikine_LMS(TE);\n",
    "sol = panda.ikine_LM(TE);"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "ebd433e8",
   "metadata": {},
   "outputs": [],
   "source": [
    "J = panda.jacob0(sol.q);\n",
    "J.shape"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "72a73a9b",
   "metadata": {},
   "outputs": [],
   "source": [
    "xd_desired = [0.1, 0.2, 0.3, 0, 0, 0];"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "57859dcd",
   "metadata": {},
   "outputs": [],
   "source": [
    "qd = np.linalg.pinv(J) @ xd_desired"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "ca53d2bc",
   "metadata": {},
   "outputs": [],
   "source": [
    "J @ qd"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "c648f201",
   "metadata": {},
   "outputs": [],
   "source": [
    "np.linalg.matrix_rank(J)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "726c3253",
   "metadata": {},
   "outputs": [],
   "source": [
    "N = linalg.null_space(J);\n",
    "N.shape\n",
    "N.T"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "bb8b34a7",
   "metadata": {},
   "outputs": [],
   "source": [
    "np.linalg.norm( J @ N[:,0])"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "6db5571f",
   "metadata": {},
   "outputs": [],
   "source": [
    "qd_0 = [0, 0, 0, 0, 1, 0, 0];"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "a105f949",
   "metadata": {},
   "outputs": [],
   "source": [
    "qd = N @ np.linalg.pinv(N) @ qd_0"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "a30edbe0",
   "metadata": {},
   "outputs": [],
   "source": [
    "np.linalg.norm(J @ qd)"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "5002a951",
   "metadata": {},
   "source": [
    "# 8.4 Force Relationships\n"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "da329089",
   "metadata": {},
   "source": [
    "## 8.4.1 Transforming Wrenches to Joint Space\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "1740b8ea",
   "metadata": {},
   "outputs": [],
   "source": [
    "tau = ur5.jacob0(ur5.q1).T @ [0, 20, 0, 0, 0, 0]"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "14f3b4fb",
   "metadata": {},
   "outputs": [],
   "source": [
    "tau = ur5.jacob0(ur5.q1).T @ [20, 0,  0, 0, 0, 0]"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "fe5f5bdc",
   "metadata": {},
   "source": [
    "## 8.4.2 Force Ellipsoids\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "7513ff13",
   "metadata": {},
   "outputs": [],
   "source": [
    "# planar2.teach(np.deg2rad([30, 40]), fellipse=True);\n",
    "planar2.plot(np.deg2rad([30, 40]), fellipse=True);"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "6545a937",
   "metadata": {},
   "source": [
    "# 8.6 Advanced Topics\n"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "e6be01a3",
   "metadata": {},
   "source": [
    "## 8.6.1 Manipulability Jacobian\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "e3831e80",
   "metadata": {},
   "outputs": [],
   "source": [
    "panda = models.ETS.Panda()\n",
    "panda.jacobm(panda.qr).T"
   ]
  }
 ],
 "metadata": {
  "jupytext": {
   "cell_metadata_filter": "-all",
   "main_language": "python",
   "notebook_metadata_filter": "-all"
  },
  "kernelspec": {
   "display_name": "dev",
   "language": "python",
   "name": "python3"
  },
  "language_info": {
   "codemirror_mode": {
    "name": "ipython",
    "version": 3
   },
   "file_extension": ".py",
   "mimetype": "text/x-python",
   "name": "python",
   "nbconvert_exporter": "python",
   "pygments_lexer": "ipython3",
   "version": "3.10.9"
  },
  "vscode": {
   "interpreter": {
    "hash": "019dbbf6354d31390a1af2a8707908a74d3d4774daf62f20d47153cfbd4b4bc4"
   }
  }
 },
 "nbformat": 4,
 "nbformat_minor": 5
}
